
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mavproxy_monitor.c
  * @author     baiyang
  * @date       2021-7-22
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mavproxy_monitor.h"

#include <common/grapilot.h>
#include <rthw.h>
#include <rtthread.h>
#include <rtdevice.h>

#include "mavproxy.h"
#include "mavproxy_dev.h"
#include "mavlink_param.h"
#include <ftp/ftp_manager.h>


#include <c_library_v2/common/mavlink.h>
#include <c_library_v2/ardupilotmega/mavlink.h>

#include <copter/fms.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/
#define TAG "MAV_Monitor"

#define EVENT_MAV_RX (1 << 0)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static char thread_mavlink_rx_stack[4096];
struct rt_thread thread_mavlink_rx_handle;

static struct rt_event _mav_rx_event;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
extern void mavlink_console_process_rx_msg(const mavlink_serial_control_t* serial_control);

static gp_err _handle_mavlink_msg(mavlink_message_t* msg, mavlink_system_t system)
{
    switch (msg->msgid) {
    case MAVLINK_MSG_ID_HEARTBEAT: {

    } break;

    case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
        if (system.sysid == mavlink_msg_param_request_read_get_target_system(msg)) {
            mavlink_param_request_read_t request_read;
            mavlink_msg_param_request_read_decode(msg, &request_read);

            char param_name[17] = {0};
            memcpy(param_name, request_read.param_id, 16);

            param_t* param = param_get_by_name(param_name);

            if (param) {
                mavlink_param_send(param);
            } else {
                // ulog_w(TAG, "get unknown parameter:%s\n", request_read.param_id);
                send_mavlink_param(param_name);
            }
        }

    } break;

    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
        if (system.sysid == mavlink_msg_param_request_list_get_target_system(msg)) {
            mavproxy_send_banner();
            mavproxy_send_event(EVENT_SEND_ALL_PARAM);
        }

    } break;

    case MAVLINK_MSG_ID_PARAM_SET: {
        if (system.sysid == mavlink_msg_param_set_get_target_system(msg)) {
            mavlink_param_set_t param_set;
            mavlink_msg_param_set_decode(msg, &param_set);

            char param_name[17] = {0};
            memcpy(param_name, param_set.param_id, 16);

            if (mavlink_param_set(param_name, param_set.param_value) == GP_EOK) {
                param_t* param = param_get_by_name(param_name);
                mavlink_param_send(param);
            } else {
                // ulog_w(TAG, "set unknown parameter:%s\n", param_set.param_id);
            }
        }

    } break;

    case MAVLINK_MSG_ID_COMMAND_LONG: {
        if (system.sysid == mavlink_msg_command_long_get_target_system(msg)) {
            mavlink_command_long_t command;
            mavlink_msg_command_long_decode(msg, &command);

            mavproxy_proc_command(&command, msg);
        }

    } break;

    case MAVLINK_MSG_ID_SERIAL_CONTROL: {
        mavlink_serial_control_t serial_control;
        mavlink_msg_serial_control_decode(msg, &serial_control);

        /* handle received msg */
        mavlink_console_process_rx_msg(&serial_control);
    } break;

    case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: {
        if (system.sysid == mavlink_msg_file_transfer_protocol_get_target_system(msg)) {
            mavlink_file_transfer_protocol_t ftp_protocol_t;

            mavlink_msg_file_transfer_protocol_decode(msg, &ftp_protocol_t);

            if (ftp_process_request(ftp_protocol_t.payload, msg->sysid, msg->compid) == GP_EOK) {

                ftp_protocol_t.target_system = msg->sysid;
                ftp_protocol_t.target_component = msg->compid;
                ftp_protocol_t.target_network = 0;

                mavlink_msg_file_transfer_protocol_encode(system.sysid, system.compid, msg, &ftp_protocol_t);

                if (mavproxy_send_immediate_msg(msg, 0) != 1) {
                    console_printf("ftp msg send err\n");
                    return GP_ERROR;
                }
            }
        }
    } break;

    case MAVLINK_MSG_ID_MANUAL_CONTROL:
    {
        mavlink_manual_control_t packet;
        mavlink_msg_manual_control_decode(msg, &packet);

        mavlink_system_t sysid_this_mav = mavproxy_get_system();

        if (packet.target != sysid_this_mav.sysid) {
            break; // only accept control aimed at us
        }

        if (packet.z < 0) { // Copter doesn't do negative thrust
            break;
        }

        uint32_t tnow = time_millis();

        mavproxy_rc_manual_override(fms.channel_roll, packet.y, 1000, 2000, tnow, false);
        mavproxy_rc_manual_override(fms.channel_pitch, packet.x, 1000, 2000, tnow, true);
        mavproxy_rc_manual_override(fms.channel_throttle, packet.z, 0, 1000, tnow, false);
        mavproxy_rc_manual_override(fms.channel_yaw, packet.r, 1000, 2000, tnow, false);

        // a manual control message is considered to be a 'heartbeat'
        // from the ground station for failsafe purposes
        
        break;
    }

    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
        mavproxy_handle_rc_channels_override(msg);
        break;

    case MAVLINK_MSG_ID_SET_MODE:
        mavproxy_handle_set_mode(msg);
        break;
    default: {
        // console_printf("unknown mavlink msg:%d\n", msg->msgid);
        return GP_ENOTHANDLE;
    } break;
    }

    return GP_EOK;
}

void mavproxy_rx_loop()
{
    static mavlink_message_t msg = {0};
    static mavlink_status_t mav_status = {0};
    static mavlink_system_t mavlink_system = {0};
    static char byte;

    mavlink_system = mavproxy_get_system();

    while (mavproxy_dev_read(_mav_dev_chan, &byte, 1, 0)) {
        /* decode mavlink package */
        if (mavlink_parse_char(0, byte, &msg, &mav_status) == 1) {
            _handle_mavlink_msg(&msg, mavlink_system);
        }
    }
}
/*------------------------------------test------------------------------------*/


